/**
 * @file optitopo.h
 * @author mzy
 * @brief optimize the topology of swarm robot
 * @date 2022-01-18
 */

#ifndef OPTOTOPO_H
#define OPTOTOPO_H

#include <vector>
#include <cmath>
#include <Eigen/Dense>
#include <algorithm>
#include <iostream>
#include <complex.h>
#include <time.h>

using namespace Eigen;

// 寻找最大fitness，注释后则寻找最小fitness
#define MAXIMIZE_FITNESS

struct Particle
{
	int dim_;							// 参数维度（position和velocity的维度）
	double fitness_;
	double *position_ = nullptr;
	double *velocity_ = nullptr;

	double *best_position_ = nullptr;
	double best_fitness_;
	double *results_ = nullptr;			// 一些需要保存出的结果
	int results_dim_ = 0;				// results_的维度

	Particle(){}

	~Particle()
	{
		if (position_) { delete[]position_; }
		if (velocity_) { delete[]velocity_; }
		if (best_position_) { delete[]best_position_; }
		if (results_) { delete[]results_; }
	}
};


class optitopo {
    public:
        //规划参数
        double Delta_t_; //每次搜索的时间间隔

        // 优化函数的各系数
        double k_con_;
        double k_fit_;
        double k_env_;

        // 环境感知参数
        double fov_max_;        //环境感知所关注的视野角
        double fov_min_;
        int fov_resolution_;    //视野范围的离散点数
        double *env_gain_ = nullptr;
        // TODO env_param
        double env_gain_perish_time_;  //环境信息增益消亡的时间
        double env_gain_perish_cof_;

        // 相机参数
        Matrix<double, 2, 4> cam_yaw_bound_; //相机运动角度范围
        double cam_yaw_step_;                //相机最大运动速度

        double Connectivity_;
        Vector4d alpha_out_;
        Vector4d alpha_in_;
        Vector4d formation_param_new_;
        Vector4d formation_param_step_;  // 队形变换参数规划的步长
        Vector4d formation_param_vel_;   // 队形参数每次变换的最大值
        double formation_param_k_form_;

        Matrix<double, 2, 4> formation_init_;
        Matrix<double, 2, 4> formation_new_;
        Matrix<double, 2, 4> formation_param_bound_;
        Matrix<double, 4, 4> theta_ij_;

        std::vector<Eigen::Vector2d> obs_means_; // 障碍物中心坐标,相对于集群队形中心 !!!必须是相对的
        std::vector<double> pi_vector_;           // 障碍物的点云数量/所占概率 TODO

        // 为了画图
        double env_potential_total_;
        double env_potential_form_;
        double Infoexe_=0.0;
        double Infogain_=0.0;
        double lambda2_ = 0.0;                     //拉普拉斯矩阵的次小特征值

        //粒子优化函数
        int particle_num_;					// 粒子个数
        int max_iter_num_;					// 最大迭代次数
        int curr_iter_;						// 当前迭代次数
        int dim_;							// 参数维度（position和velocity的维度）
        Particle *particles_ = nullptr;		// 所有粒子

        double *upper_bound_ = nullptr;					// position搜索范围上限
        double *lower_bound_ = nullptr;					// position搜索范围下限
        double *range_interval_ = nullptr;				// position搜索区间长度

        double *dt_ = nullptr;							// 时间步长
        double *wstart_ = nullptr;						// 初始权重
        double *wend_ = nullptr;						// 终止权重
        double *w_ = nullptr;							// 当前迭代权重
        double *C1_ = nullptr;							// 加速度因子
        double *C2_ = nullptr;							// 加速度因子

        double all_best_fitness_;						// 全局最优粒子的适应度值
        double *all_best_position_ = nullptr;			// 全局最优粒子的poistion
        double *cam_results_ = nullptr;						// 一些需要保存出的结果
        double *uav_results_ = nullptr;						// 一些需要保存出的结果
        int results_dim_ = 0;							// results的维度
        bool Iter_Type_ = 0;                           // 迭代分为两个阶段，0：迭代相机角度，1：迭代无人机位置

    public:
        // 默认构造函数
	    optitopo() {}
        // 构造函数
        optitopo(int dim, int particle_num, int results_dim, int fov_reso, double _fov_max, double _fov_min);
        // 析构函数
        ~optitopo();

        double GetWeight(double _theta_ij, double _alpha_i);
        double GetLambda2(Matrix<double,4,4> _mat_ij, Vector4d _alpha);
        double CalculatePotential(Vector2d _pos1, Vector2d _pos2, double _k);
        double GetEnvPotential(Matrix<double, 2, 4> _formation,bool final_flag);
        double CalculateInfoGain(double _angle, double _mid_angle);
        double GetEnvInfoGain(Vector4d _alpha);
        void  UpdateEnvInfoGain(Vector4d _alpha);
        double SolveLambda2_Analytical(Matrix4d Laplace);
        double SolveLambda2_Eigen(Matrix4d Laplace);
        double SolveLambda2_PowerIteration(Matrix4d Laplace, int iter_times, double conver_thre);
        
        // 利用粒子群算法 搜索最终结果
        bool GetOutput_pso(Vector4d _formation_param_last, Vector4d _alpha, double _Delta_t);
        // 更新队形
        void UpdateFormation(Vector4d _formation_param, Matrix<double, 2, 4> &_formation_new);
        // 将现有队形拟合为队形参数
        bool ConvertFormation2Param(Matrix<double, 2, 4> _formation_init, 
                                    Matrix<double, 2, 4> _formation_new, Vector4d &_formation_param);
        // 更新相对位置角
        void UpdateTheta(Matrix<double, 2, 4> _formation_new);
        // 在速度范围内更新队形参数
        double UpdateParamwithVel(double _param_last, double _param_new, double _param_vel);

        // PSO function
        // 初始化所有粒子参数
        void InitialAllParticles();
        // 初始化第i个粒子参数
        void InitialParticle(int i);
        // 获取双精度随机数（默认精度为0.0001）
        double GetDoubleRand(int N = 9999);
        void GetBound(double _value_now, double _upbound, double _lowbound, double _vel,
                      double &_upbound_out, double &_lowbound_out);
        // 计算该粒子的适应度值
        double GetFitness_Camera(Particle& particle);
        double GetFitness_Uav(Particle& particle);
        // 完成设定次数的迭代过程
        void GetFinalResult();
        // 更新所有粒子参数
        void UpdateAllParticles();
        // 更新第i个粒子
        void UpdateParticle(int i);
        // 获取当前迭代的权重
        void GetInertialWeight();
        
};

#endif //OPTOTOPO_H